clear
MCP=1;
LL=10;% latch length
TARGET=pi; % the desired angle of the pendulum: blancing upright
LEARNING_RATE=1.5;


P1=[5 .25 0 .1]; %TEST
%--------------------------------
% these settings cause the pendulum to swing up and slowly cross the target
% angle
% T=[1 1]; % accelleration impulse width in seconds
% ts=[0 .408];% time shifts of impulses
% Amp=[-30 30];% amplitudes of impulses
% Parameters:
% Amplitude, Period, Time Start1, Time Start 2
%--------------------------------
%% MAIN LOOP
ap_ta_times=0;
grad_inc=.01; % defines differential size when calculating gradients
P0=[5 .25 0 .1];% point 0: [A,T,Tst,Tsh]
while max(ap_ta_times)==0  % repaet operation until target is reached  
    GradP0_time=[];% GradP0 is the gradient at Point 0. It's the vector of partials: [dA dT dTst1 dTst2]
    GradP0=[];% the time vector is just to record the occurance of each maxima
    % This is the gradient with regards to the current feature, which is
    % the magnitude of the first extrema of the angular position 
    for grad_comp=1:(length(P0)+1) % lopo through the components of the gradient vector
        P1=P0;
        if grad_comp>1% first evaluate at P0
            if grad_comp==4 % don't move initial time shift ( equals 0)
                P1(grad_comp-1)=P0(grad_comp-1);
            else
                P1(grad_comp-1)=P0(grad_comp-1)+grad_inc; % increment one component of the parapeter vector by the differential increment
            end
        end
        [t,x,ap_peaks,ap_zc,ap_ta]=sim('ipend',3);% test in simulation
        P1=P0;
        %ap_peaks= 
        % AP Peak Times y(1:LL)
        % AP Peak Values y(LL+1:LL*2)
        %ap_zc=
        % AP ZC Times
        % AP Velocity at ZC times
        %ap_ta
        % AP Target acquisition times
        % AP Velocity at acquisition times

        %% Analyze
        ap_peak_times=0;
        ap_peak_vals=0;
        ap_ta_times=0;% target acquisition
        ap_ta_vel=0;% angular velocity at target acquisition
        ap_zc_times=0;% zero crossing
        ap_zc_vel=0;% angular velocity at zero crossing

        for (i=1:LL)
            idx=min(find(ap_peaks(:,i)~=0));
            if isempty(idx)
               ap_peak_times(i)=0;
               ap_peak_vals(i)=0;     
            else
                ap_peak_times(i)=ap_peaks(idx,i);
                ap_peak_vals(i)=ap_peaks(idx,i+LL);        
            end% if
            idx=min(find(ap_zc(:,i)~=0));
            if isempty(idx)
               ap_zc_times(i)=0;  
               ap_zc_vel(i)=0;
            else
                ap_zc_times(i)=ap_zc(idx,i);       
                ap_zc_vel(i)=ap_zc(idx,LL+1);
            end% if   
            idx=min(find(ap_ta(:,i)~=0));
            if isempty(idx)
               ap_ta_times(i)=0; 
               ap_ta_vel(i)=0;
            else
                ap_ta_times(i)=ap_ta(idx,i);      
                ap_ta_vel=ap_ta(idx,LL+1);
            end% if     
        end% for i
        if grad_comp==1
          [row col P0_val]=find(ap_peak_vals,1,'first'); % consider the first angular position extrema for now.  Note that there may be many extrema depending on oscillation of pendulum
        else
            [row col GradP0_val]=find(ap_peak_vals,1,'first'); % consider the first angular position extrema for now.  Note that there may be many extrema depending on oscillation of pendulum
            GradP0=[GradP0 GradP0_val- P0_val];
            GradP0_time=[GradP0_time ap_peak_times(col)];            
        end
    end% for grad_comp
    %----------------------------------------------------------------------
    %Approach target by ascending gradient.  Learning rate adjusts ascent
    %speed.  Intuition says that a too high rate will cause oscilations
    %about target. Too low will take a long time to learn.  Rate perhaps
    %should be initially fast, then decrease over time.  For now, it's
    %constant.
    P0N0=P0;
    P0N1=P0+GradP0;%*LEARNING_RATE;%
    LEARNING_RATE=exp(.5*(TARGET-P0_val));%1+.005/sqrt(sum((P0N1-P0N0).^2)); % use distance between points to adjust learning rate
    P0=P0N1*LEARNING_RATE;
    % IF MAXIMUM VELOCITY IS REACHED HERE, START NEW SEGMENT OF MANEUVER
    % IF MAXIMUM DISPLACEMENT IS REACHED, ( CART POSITION) START NEW
    % SEGMENT
    if P0(2)>1
        AREA_0=(P0(1)*P0(2))/2; % integral of accelleration pulse from 0->T=AT/2
        P0(2)=1;
        P0(1)=AREA_0*2; % preserve area of accelleration impulse
    end
    if P0(3)<0
        P0(4)=P0(4)-P0(3);
        P0(3)=0;  % initial time must be greater than or equal to 0;
    end
%     if P0(4)>1
%         P0(4)=rand;  % random num between 0 and 1;
%     end
    if P0(4)>P0(2)
        P0(4)=.5*P0(2);  % time shift of second impulse must be greater than 0, the initial condition sets it off from the first impulse so the sum >0
    end
    P0N0
    P0
 L=P0(4)+P0(2); % Period + phase shift of second impulse
Cart_Distance=(P0(1)*P0(2)*sin((pi*L)/P0(2))*sin((pi*P0(4))/P0(2))*sin((pi*(L-P0(4)))/P0(2)))/pi
    %----------------------------------------------------------------------
end% while
%% PLOT
clf
stem(ap_peak_times,ap_peak_vals,'o')
hold
stem(ap_zc_times,ap_zc_vel,'s')
stem(ap_ta_times,ap_ta_vel,'r*')
%%


(exp(6*MCP)*(log(exp(-3*MCP)+exp(3*u))-log(exp(3*u-3*MCP)+1)))/(3*(exp(6*MCP)-1))
